'''
9点标定  依赖：相机通讯模块 + 机械臂通讯模块
'''
import random
import sys
import time

import cv2
# import rospy
import numpy as np
# from dobot.srv import GetPose
# from Camrea import Carmera

# from communication.camera_MVS.my_camera import My_Camera as Carmera
from MVSCam.MVSCam_class import MVSCam as Carmera

# sys.path.append('D:\code\huoNiWeiEr\project_huoni\communication\plc_S7')
# from communication.plc_S7.HuoniPlcClient_v0 import HuoniPlcClient

import json


def sim_get_pose(ind):
    '''
    仿真 获取相对机械臂基坐标系的坐标
    :return:
    '''
    width, height = 640, 640  # mm
    ijs = []
    # for i in range(3): # 从上到下
    #     for j in range(3): # 从左到右
    for i in range(2, -1, -1):  # 从下到上
        for j in range(2, -1, -1):  # 从右到左 和cv2识别的圆点顺序一致
            ijs.append((i, j))

    y_pixe = int(height / 6 * (2 * ijs[ind][0] + 1))
    x_pixe = int(height / 6 * (2 * ijs[ind][1] + 1))
    x_robot = 2 * x_pixe + 9 * y_pixe + 100 + random.random()
    y_robot = 2 * x_pixe + 2 * y_pixe + 1030 + random.random() # 放大两倍，平移（100，100）

    # _, jis_pixe = sim_get_frame()
    # x_pixe, y_pixe = jis_pixe[ind]
    # x_robot = 2 * x_pixe + 0 * y_pixe + 100
    # y_robot = 0 * x_pixe + 2 * y_pixe + 100 # 放大两倍，平移（100，100）

    return (x_robot, y_robot)


def sim_get_frame():
    '''
    仿真 获取标定板图像  # i行j列 xy横纵坐标轴
    :return:
    '''
    width, height = 640, 640
    img = np.ones((height, width), dtype=np.uint8) * 255

    # 模拟像素坐标
    jis_pixe = []
    for i in range(3):
        for j in range(3):
            y = int(height / 6 * (2 * i + 1))
            x = int(height / 6 * (2 * j + 1))
            jis_pixe.append((x, y))
    #
    for j, i in jis_pixe:
        cv2.circle(img, (j, i), int(height / 12), [0, 0, 0], -1, )
        # cv2.circle(img, (y, x), 4, [255, 255, 255], -1, )

    # cv2.imwrite("9p.jpg", img)
    return img


class Calibration:
    def __init__(self, device):
        self.w = 3
        self.h = 3
        self.b = 0.04
        self.minArea = 950 # 图像最小圆点面积
        self.maxArea = 50000
        self.cam_corners = []
        self.arm_points = []
        self.cam = Carmera(device['ip'])
        # self.cmd_get_pose = rospy.ServiceProxy("/DobotServer/GetPose", GetPose)
        self.isSim = False  # 是否仿真
        if self.isSim:
            self.sim_ind = 0
        self.count = 0 # 机械臂坐标序号

        # self.arm_id = arm_id
        # self.plc = HuoniPlcClient() # 用来读取机械臂坐标
        self.plc = HuoniPlcClientBase(device['addressTable'])  # 用来读取机械臂坐标
        self.plc.connect()
        product, camid = device['product'], device['index']
        self.save_file = f'9p_calib_{product}_cam{camid}.json'  # 结果文件


    def find_circles_grid(self, color_image):
        """
        查找圆心坐标
        :param color_image:
        :return: 是否找到True or False, 图像上的圆心坐标
        """
        params = cv2.SimpleBlobDetector_Params()
        params.filterByArea = True
        params.maxArea = self.maxArea
        params.minArea = self.minArea
        params.minDistBetweenBlobs = 10
        blob_detector = cv2.SimpleBlobDetector_create(params)
        ret_find_grid, corners = cv2.findCirclesGrid(image=color_image, patternSize=(self.w, self.h),
                                                     flags=(cv2.CALIB_CB_SYMMETRIC_GRID + cv2.CALIB_CB_CLUSTERING),
                                                     blobDetector=blob_detector)
        self.cam_corners = corners
        return ret_find_grid

    def get_pose(self):
        '''
        获取机械臂发送的 xy
        :return:
        '''
        # rospy.wait_for_service("/DobotServer/GetPose")
        # alarms_pose = self.cmd_get_pose()
        # return [alarms_pose.x, alarms_pose.y]

        if self.isSim:
            (x, y) = sim_get_pose(self.sim_ind)
            self.sim_ind += 1
            return (x, y)
        else:
            pass # 获取机械臂坐标

            # # 手动录入
            # x_arm, y_arm = None, None
            # while not x_arm and not y_arm:
            #     xy_arm_str = input(f'请输入第 {self.count + 1} 个机械臂坐标xy,  例如: 21.3  40.6\n')
            #     try:
            #         x_arm, y_arm = map(float, xy_arm_str.split(' '))
            #     except Exception as e:
            #         print('输入格式错误')
            #         print(e)
            #
            # print(f'x_arm y_arm {x_arm} {y_arm}')
            # return [x_arm, y_arm]


            # 从PLC获取
            # if self.arm_id == 4: # 点位4
            #     x,y,u = self.plc.get_xyu_placeScrew()
            # elif self.arm_id == 6: # 点位6 腕带
            #     x, y, u = self.plc.get_xyu_placeStrap()
            # elif self.arm_id == 3:
            #     x, y, u = self.plc.get_xyu_placeCable()
            print('getting arm xy...')
            x, y, u = self.plc.get_xyu()
            print(f'x_arm y_arm u_degree {x} {y} {u}')
            return [x, y]


    def show(self, color_image, ret_find_grid):
        print('cam_corners (j,i)')
        print(self.cam_corners)
        cv2.drawChessboardCorners(color_image, (self.w, self.h), self.cam_corners, ret_find_grid)
        image_show = cv2.resize(color_image, dsize=None, fx=0.25, fy=0.25)
        cv2.imshow('camera image corners ', 255 - image_show)
        # cv2.waitKey(10000)
        cv2.waitKey(1000)

    def file_write(self, mtx, cam_corners, arm_points):
        # def file_read(camera_intrinsic_calibration_path):  # 读取标定文件中保存的结果（相机内参矩阵和畸变系数）到对应的变量中
        fs = cv2.FileStorage('./matrix.yaml', cv2.FileStorage_WRITE)
        fs.write('cameraMatrix', mtx)

        res = {}
        res['Matrix'] = mtx.tolist()
        res['Cam_jis'] = cam_corners.squeeze().tolist()
        res['Arm_xys'] = arm_points


        with open(self.save_file, 'w') as f:
            json.dump(res, f)
        print(f'结果保存至 {self.save_file}')
        # 备份
        self.bp = self.save_file[:-5] + str(time.time()) +'.json'
        with open(self.bp, 'w') as f:
            json.dump(res, f)
        print(f'结果备份至 {self.bp}')

    def main(self):
        self.count = 0
        ret_find_grid =False # 采集相机坐标成功
        print(' 1 空格：采集相机坐标\n 2 回车：采集机械臂坐标\n 3 esc: 计算保存结果')

        while True:
            # color_frame, _ = self.cam.get_frame()
            # color_image = np.asanyarray(color_frame.get_data())
            # color_image = self.cam.getFrame()
            if self.isSim:
                color_image = sim_get_frame()
            else:
                color_image = self.cam.getFrame()
            image_show = cv2.resize(color_image, dsize=None, fx=0.25, fy=0.25)
            cv2.imshow('camera image', image_show)
            kvalue = cv2.waitKey(50) & 0xFF
            # 1 采相机数据，
            if kvalue == 32: # 空格, 1 采相机数据，
                ret_find_grid = self.find_circles_grid(color_image)  # 获取像素坐标
                self.show(color_image, ret_find_grid)

            # 2 采机械臂数据 （9次）
            elif kvalue == 10 or kvalue == 13:  # 换行 回车, 2 采机械臂数据 （9次）
                if not ret_find_grid :
                    print(f'未成功采集相机坐标。请按空格键，采集相机坐标')
                    continue

                arm_point = self.get_pose() # 获取机械臂坐标

                # 避免重复录入
                if len(self.arm_points)>0:
                    cur_x, cur_y = arm_point
                    pre_x, pre_y = self.arm_points[-1]
                    diff = (cur_x-pre_x)**2 + (cur_y-pre_y)**2
                    if diff < 3**2:
                        print('机械臂移动距离小于3mm, 不录入该坐标')
                        continue

                print(f'第 {self.count+1} 个： (x_arm y_arm) {arm_point}')
                self.arm_points.append(arm_point)  # 记录机械臂发送的一个 xy 数据
                self.count += 1

            # 3 计算M
            elif kvalue == 27:  # esc, 3 计算M
                self.cam.close()
                if not ret_find_grid and self.count !=9:
                    print(f'未成功采集相机坐标或机械臂坐标 ')
                    print(f'ret_find_grid {ret_find_grid}  count {self.count}')
                    break

                m, _ = cv2.estimateAffine2D(np.array(self.cam_corners), np.array(self.arm_points))
                # print(np.array(self.cam_corners), np.array(self.arm_points))
                self.file_write(m, self.cam_corners, self.arm_points)
                break

sys.path.append(r'D:\code\huoNiWeiEr\project_huoni_v2\neith_fyluoding\plc_S7')
from HuoniPlcClient import HuoniPlcClientBase
from HuoniPlcClient import config as plc_config

devices = {
    'p1': # 点位1
        {
            'product': 'zhihe',# 物料名
            'index': 1,
            'xlh': 'DA1851926', # 相机序列号
            'ip': '192.168.0.112', #相机ip

            # 'algorithm':get_seg_zhihe_cam1, # 算法
            'addressTable':plc_config.LoadEmptyCarton, # plc地址表
            'isWaitPlc': False,
        },

    'p2':  # 点位2
        {
            'product': 'dianchi',  # 物料名
            'index': 2,
            'xlh': 'DA1851863',  # 相机序列号
            'ip': '192.168.0.110',  # 相机ip

            # 'algorithm': get_seg_dianchi_cam2,  # 算法
            'addressTable': plc_config.PlaceBat,  # plc地址表
            'isWaitPlc': False,
        },

    'p3':  # 点位3
        {
            'product': 'cable',  # 物料名 数据线
            'index': 3,
            'xlh': 'DA1851932',  # 相机序列号
            'ip': '192.168.0.113',  # 相机ip

            # 'algorithm': get_seg_cable_cam3,  # 算法
            'addressTable': plc_config.PlaceCable,  # plc地址表
            'isWaitPlc': False,
        },
    'p4':  # 点位4
        {
            'product': 'screw',  # 物料名 螺钉
            'index': 4,
            'xlh': 'DA1851883',  # 相机序列号
            'ip': '192.168.0.111',  # 相机ip

            # 'algorithm': get_seg_screw_cam4,  # 算法
            'addressTable': plc_config.PlaceScrew,  # plc地址表
            'isWaitPlc': False,
        },
    'p5':  # 点位5
        {
            'product': 'instruction',  # 物料名 说明书
            'index': 5,
            'xlh': 'DA1851912',  # 相机序列号
            'ip': '192.168.0.115',  # 相机ip

            # 'algorithm': get_detect_instruction_cam5,  # 算法
            'addressTable': plc_config.PlaceInstruction,  # plc地址表
            'isWaitPlc': False,
        },
    'p6':  # 点位6
        {
            'product': 'strap',  # 物料名 腕带
            'index': 6,
            'xlh': 'DA1851911',  # 相机序列号
            'ip': '192.168.0.116',  # 相机ip

            # 'algorithm': get_seg_strap_cam6,  # 算法
            'addressTable': plc_config.PlaceStrap,  # plc地址表
            'isWaitPlc': False,
        },
    'p7':  # 点位7
        {
            'product': 'phone',  # 物料名 主机
            'index': 7,
            'xlh': 'DA1851958',  # 相机序列号
            'ip': '192.168.0.117',  # 相机ip

            # 'algorithm': get_seg_phone_cam7,  # 算法
            'addressTable': plc_config.PlacePhone,  # plc地址表
            'isWaitPlc': False,
        },


}
if __name__ == '__main__':


    device = devices['p4'] # 'p3' 'p4' 'p6'
    # ip = '192.168.0.113' # 点位3 数据线
    # # ip = '192.168.0.111'  # 点位4 螺钉 充电器  DA1851883
    # # ip = '192.168.0.115'  # 点位5 说明书 DA1851912
    # # ip = '192.168.0.116'  # 点位6 腕带 DA1851911  exp18
    # # ip = '192.168.0.117'  # 点位7 主机 DA1851958
    # # arm_id = 6
    # addressTable = plc_config.PlaceCable # 点位3

    # ip = device['ip']


    calib = Calibration(device)
    calib.main()

    # sim_get_frame()
